/**
  ******************************************************************************
  * @file           : super_capacitor_drv.cpp
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/4/22
  ******************************************************************************
  */



#include "super_capacitor_drv.h"

SuperCapState super_cap_state = READY; //电容状态

Super_Cap_t superCap = {};

uint8_t is_use_capacitor = 0;

//float chassis_power_Judge;
extern receive_judge_t judge_rece_mesg;

float Power_pid;
float Power_f;

/*
 * 计算放电功率大小
 */
float SC_power_cacl()
{
    /*
     * 电容功率环
     * 缓冲能量PID + 前馈调节
     *
     *                  抽象电容板
     *      -———————————————————————————————-
     *      |                               |
     *  <————————<————-                     |
     * V_SC |         | I_IN_SC             |
     *      |         ↑                     |
     *  ——>——————>————|                     |
     * V_IN |         ↓ I_CHASSIS           |
     *      |         |                     |
     *      -—————————|—————————————————————-
     *                ↓ Motors
     *
     *  电容如何知道是否应该开始放电？
     *  1. 使用电容PID
     *     当缓冲能量减少时，说明电池放电功率增加，当使用缓冲能量较多超过阈值super_ca_ref时，应开启计算超级电容应放出功率
     *     反之，当缓冲能量剩余低于阈值时，说明裁判系统限制功率未充分利用，考虑给超级电容充电，同时PID避免缓冲能量快速下降导致降速
     *  2. 增加前馈
     *     前馈重要作用是增加响应速度，当底盘功率未到限制功率时，更快给电容充电
     */
    /******  获取裁判系统数据  ******/
    static pid super_cap_pid(1.0f, 0.1f, 2.0f, 0.f, 0.f);
    float chassis_power_limit = judge_rece_mesg.robot_status_data.chassis_power_limit;
    float chassis_power = (float)judge_rece_mesg.power_heat_data.chassis_power;
    float chassis_power_buffer = judge_rece_mesg.power_heat_data.chassis_power_buffer;
    float Power_charge=0;

    /****** PID + 前馈  ******/

    Power_f = chassis_power_limit - superCap.power_fb.p_wheel;


    //电容放电时
    if(Power_f<=0) {
        super_cap_pid.pid_calculate(55 / Buffer_MAX, chassis_power_buffer / Buffer_MAX);

        //当前缓冲能量chassis_power_buffer小于参考缓冲能量super_cap_ref时(说明底盘功率较大，开始使用缓冲能量了)，此时超级电容开始放电

        Power_pid = -super_cap_pid.out * Power_delta;
        if(Power_pid>0)
        {
            Power_pid=0;
        }
        Power_charge = Forward_K * Power_f + Power_pid;
        //充放电功率(>0为给电容充电，反之放电)
    }

        //电容充电时
    else {
        super_cap_pid.pid_calculate(super_cap_ref / Buffer_MAX, chassis_power_buffer / Buffer_MAX);
        Power_pid = -super_cap_pid.out * 40;
        Power_charge = Power_f + Power_pid;
    }

    // 充电限幅
    if(Power_charge > (float)judge_rece_mesg.robot_status_data.chassis_power_limit + 20)
    {
        Power_charge = (float)judge_rece_mesg.robot_status_data.chassis_power_limit + 20;
    }

    // 电容放电功率不能超过底盘轮耗电功率 否则多放出的电会全部木大,反向放给电源管理,白白浪费
    else if (Power_charge < -superCap.power_fb.p_wheel-20)
        Power_charge = -superCap.power_fb.p_wheel-20;

    return Power_charge;
}

/*
 * 超级电容管控
 */
float set_power_charge;
void super_cap_control(void)
{
    /*
     * 新电容控制程序
     * autoSuperFlag仅代表操作手对电容放电与否的控制
     * 操作手不需要也做不到对电容充电与否进行控制 这部分控制（包括防止抢电）应该是电控的任务
     *
     * autoSuperFlag用于追击等需求时开启超级电容，而AutoC是为了飞坡时开启超级电容的标志位
     * AutoC发送给电容控制板，使其短时间内能更快响应放电需求
     *
     * 底盘功率环需要3个输入：缓冲能量 功率上限 实时功率反馈
     * 使用电容放电:       缓冲能量         固定为60J
     *                  底盘功率上限      固定为120w   由电容功率环来保证不会超功率
     *                  实时功率反馈      由电容板测量提供（SC_get_power_wheel()）
     * 不使用电容放电:     缓冲能量         从裁判系统获取
     *                  底盘功率上限      从裁判系统获取
     *                  实时功率反馈      依旧由电容板测量提供（SC_get_power_wheel()）  如果从裁判系统获取会有抢电问题
     */


    set_power_charge = SC_power_cacl();//计算充放电能量

    /******  电容状态切换控制  ******/
    switch (super_cap_state)
    {
        case EMPTY:
        {   // 电容电放完了 不能放电
            set_power_charge = set_power_charge < 0 ? 0 : set_power_charge;
            if(!superCap.scSet.enable && superCap.scSet.set_power_charge > 0) superCap.scSet.enable = 1;
            break;
        }
        case READY:
        {   // 电容就绪 可充可放
            if(!superCap.scSet.enable && superCap.scSet.set_power_charge > 0) superCap.scSet.enable = 1;
            break;
        }
        case FULL:
        {    // 电容电满 不需要继续充电
            set_power_charge = set_power_charge > 0 ? 0 : set_power_charge;
            if(superCap.scSet.enable) superCap.scSet.enable = 0;
            break;
        }
        default:
        {
            set_power_charge = 0;
            break;
        }
    }

    //未开启使用超级电容，不放电仅充电
    if(is_use_capacitor == 0) {
        set_power_charge = set_power_charge < 0 ? 0 : set_power_charge;
    }

    superCap.scSet.set_power_charge = set_power_charge;
//    superCap.scSet.set_power_charge = 0;
}

float SC_get_power_wheel(){
    return superCap.power_fb.p_wheel;
}